import rclpy
from rclpy.node import Node
from chapt4_interfaces.srv import FaceDetector
from ament_index_python.packages import get_package_share_directory
from cv_bridge import CvBridge
import cv2
import face_recognition
import time
class FaceDectorNode(Node):
    def __init__(self):
        super().__init__('face_detection_node')
        self.bridge=CvBridge()
        self.service_=self.create_service(FaceDetector,'/face_detect',self.detect_face_timecallback)
        self.default_image_path=get_package_share_directory(
        'demo_python_service') + '/resource/trump.jpeg'
        self.upsample_times=1
        self.model="CNN"
    def detect_face_timecallback(self,request,response):
        if request.image.data:
            cv_image=self.bridge.imgmsg_to_cv2(request.image)
        else:
            cv_image=cv2.imread(self.default_image_path)
        start_time=time.time()
        self.get_logger().info('start')
        face_location=face_recognition.face_locations(cv_image,number_of_times_to_upsample=self.upsample_times,model=self.model)
        end_time=time.time()
        self.get_logger().info(f'finish,spend{end_time-start_time}')
        response.number=len(face_location)
        response.use_time=end_time-start_time
        for top,right,bottom,left in face_location:
            response.top.append(top)
            response.right.append(right)
            response.bottom.append(bottom)
            response.left.append(left)

        return response
def main():
    rclpy.init()
    node=FaceDectorNode()
    rclpy.spin(node)
    rclpy.shutdown()
    
        
        